#pragma  warning (disable:4244) // loss of precision when converting double to float
#pragma  warning (disable:4305) // truncation from 'double' to 'const float'

#include "meshutil.h"

#include <cmath>

template<typename T>
inline T min(T a, T b)
{
	if (a<b) return a; else return b;
}

template<typename T>
inline T min3(T a, T b, T c)
{
	if (a<b) return min(a,c); else return min(b,c);
}


MyREAL angle(OpenMesh::Vec3f& pointA, OpenMesh::Vec3f& pointB, OpenMesh::Vec3f& pointC)
{
	MyREAL sinTheta = cross(pointA-pointB, pointC-pointB).norm() / ((pointA-pointB).norm()*(pointC-pointB).norm());
	MyREAL cosTheta = dot(pointA-pointB, pointC-pointB);
	MyREAL theta = asin(sinTheta)*PITOGRAD; 
	if (cosTheta<0)
	{
		theta = 360 - theta;
	}
	return theta;
}


TriMesh::FaceHandle addFaceSimple(TriMesh& mesh, TriMesh::VertexHandle a, TriMesh::VertexHandle b, TriMesh::VertexHandle c)
// adds a new face to the mesh, if the verteces are already part of the mesh
{
	TriMesh::FaceHandle fh = mesh.add_face(a, b,c );
	if (fh.idx()<0) // invalid face
		fh = mesh.add_face(b, a,c ); // reorder the points
	return fh;
} // addFaceSimple


void addFace(TriMesh& mesh, TriMesh::VertexHandle a, TriMesh::VertexHandle b, TriMesh::VertexHandle c)
// adds a new face to the mesh, if the verteces are already part of the mesh
{
	static std::vector<TriMesh::VertexHandle>  face_vhandles(3);
// compute quality
	OpenMesh::Vec3f pointA = mesh.point(a);
	OpenMesh::Vec3f pointB = mesh.point(b);
	OpenMesh::Vec3f pointC = mesh.point(c);

	MyREAL angleA = angle(pointC, pointA, pointB);
	MyREAL angleB = angle(pointA, pointB, pointC);
	MyREAL angleC = angle(pointB, pointC, pointA);
	MyREAL ab = (pointA-pointB).norm();
	MyREAL bc = (pointB-pointC).norm();
	MyREAL ca = (pointC-pointA).norm();

	OpenMesh::Vec3f av = pointA-pointC;
	OpenMesh::Vec3f bv = pointB-pointC;
	// http://en.wikipedia.org/wiki/Circumcenter
	MyREAL circumdiameter = av.norm()*bv.norm()*(av-bv).norm() / cross(av,bv).norm();
//	double quality = min3(angleA, angleB, angleC);
	MyREAL quality = min3(ab, bc, ca) / circumdiameter; // bad if quality < Threshold
//	cout<< "Adding" << a <<"  " << b << "  "<< c<<"  "<<quality<<endl;

//	if (quality<threshold) addFaceToListofFutureImprovements;
	addFaceSimple(mesh, a,b,c);
} // addFace

// deletes the triangle identified by <f> and adds a new tetrahedron to the mesh formed by the vertices of the deleted tringle and the new point <P>
void extendMesh(TriMesh& mesh, TriMesh::FaceHandle& fh, OpenMesh::Vec3f& P, bool runGarbageCollector)
{

	OpenMesh::VertexHandle newP = mesh.add_vertex(P);
	TriMesh::ConstFaceVertexIter cfvIt;
	cfvIt = mesh.cfv_iter(fh);
	OpenMesh::VertexHandle pointA = cfvIt.handle();
	OpenMesh::VertexHandle pointB = (++cfvIt).handle();
	OpenMesh::VertexHandle pointC = (++cfvIt).handle();
	mesh.delete_face(fh, false);
	addFace(mesh, pointA, pointB, newP);
	addFace(mesh, pointB, pointC, newP);
	addFace(mesh, pointC, pointA, newP);
	if (runGarbageCollector)
	{
		mesh.garbage_collection();
	}
}

TriMesh createTetra(MyREAL edgelen)
{
	TriMesh mesh;
	TriMesh::VertexHandle vhandle[4];
	vhandle[0] = mesh.add_vertex(TriMesh::Point(edgelen/2, 0,  -edgelen*sqrt(2.0)/4.0));
	vhandle[1] = mesh.add_vertex(TriMesh::Point(-edgelen/2, 0,  -edgelen*sqrt(2.0)/4.0));
	vhandle[2] = mesh.add_vertex(TriMesh::Point( 0,  -edgelen/2,  edgelen*sqrt(2.0)/4.0));
	vhandle[3] = mesh.add_vertex(TriMesh::Point(0,  edgelen/2,  edgelen*sqrt(2.0)/4.0));

	//std::vector<TriMesh::VertexHandle>  face_vhandles;

	addFace(mesh, vhandle[1], vhandle[2], vhandle[0]);
	addFace(mesh, vhandle[0], vhandle[2], vhandle[3]);
	addFace(mesh, vhandle[3], vhandle[2], vhandle[1]);
	addFace(mesh, vhandle[1], vhandle[0], vhandle[3]);

	return mesh;
}// TriMesh createTetra()
